#include <stdint.h>
#include <stdio.h>
#include "rf_driver_hal_vtimer.h"
#include "master_ctl.h"
#include "usr_radio_cfg.h"
#include "cassert.h"
#include "ull_chan.h"
#include "lll_chan.h"

#define BLE_ADV_ACCESS_ADDRESS  (uint32_t)(0x8E89BED6)
#define STARTING_CHANNEL        (uint8_t)(24)   // RF channel 22
#define END_CHANNEL             (uint8_t)(26)


static uint8_t channel = STARTING_CHANNEL;   /* Start Channel */
static uint8_t dumpData[MAX_PACKET_LENGTH];
static uint8_t scanData[MAX_PACKET_LENGTH];
static uint8_t txConReqData[MAX_PACKET_LENGTH];
static uint8_t rxConRspData[MAX_PACKET_LENGTH];
static uint8_t txConData[MAX_PACKET_LENGTH];
static uint8_t rxConData[MAX_PACKET_LENGTH];

static ActionPacket dump; 
static ActionPacket scan; 
static ActionPacket con_req; 
static ActionPacket con_rsp; 
static ActionPacket con_tx; 
static ActionPacket con_rx; 
static uint64_t next_wakeup_time = 0;
static uint64_t con_timestap = 0;
static uint8_t con_sta = STA_IDLE;
static uint32_t lost_packet_count  = 0;
static uint16_t cur_event_num = 0;

static void update_con_chan(void)
{
  uint8_t ch_map[5];
  uint8_t chan_count = ull_chan_map_get(ch_map);
  channel = lll_chan_sel_2(cur_event_num, 0x305F, ch_map,chan_count); 
  //COMPrintf("Channel:%d \r\n", channel);
  RADIO_SetChannel(STATE_MACHINE_0, channel, 0);
}

static void __do_scan(void)
{
  channel = STARTING_CHANNEL;
  RADIO_SetChannel(STATE_MACHINE_0, channel, 0);
  next_wakeup_time = HAL_VTIMER_GetCurrentSysTime();
  if(SCAN_INTERVAL < SCAN_PERIOD ){
      next_wakeup_time +=  RADIO_TIME_UNIT;
  }
  else{
      next_wakeup_time +=  (SCAN_INTERVAL - SCAN_PERIOD);
  }
  scan.WakeupTime = next_wakeup_time; 
  uint32_t time_out = (uint32_t)(SCAN_PERIOD*2.4414);
  RADIO_SetGlobalReceiveTimeout(time_out);
  
}


static uint8_t dumpConditionRoutine(ActionPacket* p)
{
  next_wakeup_time = HAL_VTIMER_GetCurrentSysTime();
  next_wakeup_time +=  RADIO_TIME_UNIT;
  scan.WakeupTime = next_wakeup_time; 
  uint32_t time_out = (uint32_t)(SCAN_PERIOD*2.4414);
  RADIO_SetGlobalReceiveTimeout(time_out);
  
  RADIO_SetReservedArea(&scan);
  return TRUE;   
}

static uint8_t dumpDataRoutine(ActionPacket* p,  ActionPacket* next)
{
  return TRUE;
}

static uint8_t scanConditionRoutine(ActionPacket* p)
{  
  if((p->status & BLUE_STATUSREG_PREVTRANSMIT) == 0) {
    /* received a packet */
    if((p->status & BLUE_INTERRUPT1REG_RCVOK) != 0) {
      // send connection request
      RADIO_SetGlobalReceiveTimeout(DEF_RADIO_RECEIVE_TIMEOUT);
      uint64_t cur_time;
      con_timestap = TIMER_GetAnchorPoint(&cur_time);
      //printf("con_timestap:%lld \r\n", con_timestap);
      return TRUE;
    }
  }
  
  __do_scan();
  return FALSE;
} 

static uint8_t scanDataRoutine(ActionPacket* p,  ActionPacket* next)
{
  return TRUE;
}


static uint8_t conReqConditionRoutine(ActionPacket* p)
{
  return TRUE;   
} 

static uint8_t conReqDataRoutine(ActionPacket* p,  ActionPacket* next)
{
  return TRUE;
}

static void __do_create_connection(void)
{
  // 
  next_wakeup_time = con_timestap + (3 * RADIO_TIME_UNIT);
  con_rx.WakeupTime = next_wakeup_time - SLAVE_TIME_OFFSET; 
  con_rx.next_false = &con_rx;
  RADIO_SetReservedArea(&con_rx);
  lost_packet_count = 0;
  cur_event_num = 0;
  update_con_chan();
}

static uint8_t conRspConditionRoutine(ActionPacket* p)
{  
  __do_create_connection();
  return TRUE;
} 

static uint8_t conRspDataRoutine(ActionPacket* p,  ActionPacket* next)
{
  COMPrintf("on conencted\r\n");
  return TRUE;
}

static uint8_t __lost_one_packet(ActionPacket* p)
{
  lost_packet_count++;
  cur_event_num++;
  if(lost_packet_count < CON_TIMEOUT){
    next_wakeup_time += 2*RADIO_TIME_UNIT;
    con_rx.WakeupTime = next_wakeup_time - SLAVE_TIME_OFFSET;; 
    con_rx.next_false = &con_rx;
    RADIO_SetReservedArea(&con_rx);
    update_con_chan();
    COMPrintf("-");
  }else{
    con_rx.next_false = &scan;
    RADIO_SetReservedArea(&con_rx);
    COMPrintf("on disconencted\r\n");
    cur_event_num = 0;
    __do_scan();
  }
  return FALSE;
}

static uint8_t __do_rec_packet(ActionPacket* p)
{
  lost_packet_count = 0;
  uint64_t cur_time;
  cur_event_num++;
  con_timestap = TIMER_GetAnchorPoint(&cur_time);
  //next_wakeup_time = STU_value + 2*RADIO_TIME_UNIT;
  return TRUE;
}

static uint8_t conRxConditionRoutine(ActionPacket* p)
{  
  if((p->status & BLUE_STATUSREG_PREVTRANSMIT) == 0) {
    /* received a packet */
    if((p->status & BLUE_INTERRUPT1REG_RCVOK) != 0) {  
      __do_rec_packet(p);
      return TRUE;
    }
  }
  
  __lost_one_packet(p);
  return FALSE;
}


static uint8_t conTxConditionRoutine(ActionPacket* p)
{  
   next_wakeup_time = con_timestap + 2*RADIO_TIME_UNIT;
   con_rx.WakeupTime = next_wakeup_time - SLAVE_TIME_OFFSET;
   RADIO_SetReservedArea(&con_rx);
   update_con_chan();
   return TRUE;
} 

static uint8_t conTxDataRoutine(ActionPacket* p,  ActionPacket* next)
{
  return TRUE;
}



static uint8_t conRxDataRoutine(ActionPacket* p,  ActionPacket* next)
{
  return TRUE;
}


static void init_action_packet(void)
{
  next_wakeup_time = HAL_VTIMER_GetCurrentSysTime() + RADIO_TIME_UNIT * 2;;
    /* Build dump Packet */
  dump.StateMachineNo = STATE_MACHINE_0;
  dump.ActionTag = PLL_TRIG  | TIMER_WAKEUP | TIMESTAMP_POSITION;
  dump.WakeupTime = next_wakeup_time;                
  dump.MaxReceiveLength = 1;                    /* Not applied for TX */
  dump.data = dumpData;                         /* Data to send */
  dump.next_true = &scan;                  /* Pointer to the next Action Packet*/
  dump.next_false = &scan;                       /* Null */   
  dump.condRoutine = dumpConditionRoutine;          /* Condition routine */
  dump.dataRoutine = dumpDataRoutine;               /* Data routine */
  
    /* Call this function before execute the action packet */
  RADIO_SetReservedArea(&dump);
  
  /* Build scan Packet */
  scan.StateMachineNo = STATE_MACHINE_0;
  scan.ActionTag = PLL_TRIG | TIMER_WAKEUP | TIMESTAMP_POSITION;         
  scan.MaxReceiveLength = 0xFF;                    /* Not applied for TX */
  scan.data = scanData;                         /* Data to send */
  scan.next_true = &con_req;                  /* Pointer to the next Action Packet*/
  scan.next_false = &scan;                       /* Null */   
  scan.condRoutine = scanConditionRoutine;          /* Condition routine */
  scan.dataRoutine = scanDataRoutine;               /* Data routine */
  
    /* Call this function before execute the action packet */
  RADIO_SetReservedArea(&scan);
  
  
    /* Build con_req Packet */
  con_req.StateMachineNo = STATE_MACHINE_0;
  con_req.ActionTag = TXRX;
  //con_req.WakeupTime = WAKEUP_TIME;                
  con_req.MaxReceiveLength = 0;                    /* Not applied for TX */
  con_req.data = txConReqData;                         /* Data to send */
  con_req.next_true = &con_rsp;                  /* Pointer to the next Action Packet*/
  con_req.next_false = &scan;                     /* Null */   
  con_req.condRoutine = conReqConditionRoutine;          /* Condition routine */
  con_req.dataRoutine = conReqDataRoutine;               /* Data routine */
  
    /* Call this function before execute the action packet */
  RADIO_SetReservedArea(&con_req);
  txConReqData[0] = 0;
  txConReqData[1] = DEF_TEST_LEN;
  
  
      /* Build con_req Packet */
  con_rsp.StateMachineNo = STATE_MACHINE_0;
  con_rsp.ActionTag = TIMESTAMP_POSITION;
  //con_rsp.WakeupTime = WAKEUP_TIME;                
  con_rsp.MaxReceiveLength = 0xff;                    /* Not applied for TX */
  con_rsp.data = rxConRspData;                         /* Data to send */
  con_rsp.next_true = &con_rx;                  /* Pointer to the next Action Packet*/
  con_rsp.next_false = &con_rx;                     /* Null */   
  con_rsp.condRoutine = conRspConditionRoutine;          /* Condition routine */
  con_rsp.dataRoutine = conRspDataRoutine;               /* Data routine */
  
    /* Call this function before execute the action packet */
  RADIO_SetReservedArea(&con_rsp);
  
      /* Build con_req Packet */
  con_rx.StateMachineNo = STATE_MACHINE_0;
  con_rx.ActionTag =  TIMER_WAKEUP | TIMESTAMP_POSITION | PLL_TRIG;
  //con_rx.WakeupTime = WAKEUP_TIME;                
  con_rx.MaxReceiveLength = 0xff;                    /* Not applied for TX */
  con_rx.data = rxConData;                         /* Data to send */
  con_rx.next_true = &con_tx;                  /* Pointer to the next Action Packet*/
  con_rx.next_false = &con_rx;                     /* Null */   
  con_rx.condRoutine = conRxConditionRoutine;          /* Condition routine */
  con_rx.dataRoutine = conRxDataRoutine;               /* Data routine */
  
    /* Call this function before execute the action packet */
  RADIO_SetReservedArea(&con_rx);

  
      /* Build con_req Packet */
  con_tx.StateMachineNo = STATE_MACHINE_0;
  con_tx.ActionTag =  TXRX ;
  //con_tx.WakeupTime = WAKEUP_TIME;                
  con_tx.MaxReceiveLength = 0;                    /* Not applied for TX */
  con_tx.data = txConData;                         /* Data to send */
  con_tx.next_true = &con_rx;                  /* Pointer to the next Action Packet*/
  con_tx.next_false = &con_rx;                     /* Null */   
  con_tx.condRoutine = conTxConditionRoutine;          /* Condition routine */
  con_tx.dataRoutine = conTxDataRoutine;               /* Data routine */
  txConData[0] = 0;
  txConData[1] = DEF_TEST_LEN;
  
    /* Call this function before execute the action packet */
  RADIO_SetReservedArea(&con_tx);
}


void master_radio_init(void)
{
  ull_chan_reset();
  RADIO_Init();

  init_action_packet();
  
  /* Channel map configuration */
  uint8_t map[5]= {0xFF,0xFF,0xFF,0xFF,0xFF};
  RADIO_SetChannelMap(STATE_MACHINE_0, &map[0]);
  
  /* Setting of channel and the channel increment*/
  RADIO_SetChannel(STATE_MACHINE_0, channel, 0);

  /* Sets of the NetworkID and the CRC.*/
  RADIO_SetTxAttributes(STATE_MACHINE_0, BLE_ADV_ACCESS_ADDRESS, 0x555555);
  
  /* Configures the transmit power level */
  RADIO_SetTxPower(MAX_OUTPUT_RF_POWER);
  LL_PWR_SetSMPSOutputLevel(LL_PWR_SMPS_OUTLVL_1V95);
  
  RADIO_SetPhy(STATE_MACHINE_0, PHY_2M);
  RADIO_SetBackToBackTime(BACK_TO_BACK_TIME);
  RADIO_SetGlobalReceiveTimeout(DEF_RADIO_RECEIVE_TIMEOUT);
  
  /* Call this function for the first action packet to be executed */
  RADIO_MakeActionPacketPending(&dump);
}



